#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/defines.h>

#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <mqueue.h>

extern "C" __EXPORT int hello_main(int argc, char *argv[]);

int open_serial(const char *dev)
{

	int rate = B115200;

	// open uart
	int fd = px4_open(dev, O_RDWR | O_NOCTTY);
	int termios_state = -1;

	if (fd < 0) {
		PX4_ERR("failed to open uart device!");
		return -1;
	}

	// set baud rate
	struct termios config;
	tcgetattr(fd, &config);


	config.c_cflag &= ~CSIZE;
	config.c_cflag |= CS8;         // 8数据位
	config.c_cflag &= ~PARENB;     // 无奇偶校验
	config.c_cflag &= ~CSTOPB;     // 1停止位

	config.c_cflag &= ~CRTSCTS;    // 关闭硬件流

	/* Set baud rate */
	if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {
		PX4_WARN("ERR SET BAUD %s: %d\n", dev, termios_state);
		px4_close(fd);
		return -1;
	}

	if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {
		PX4_WARN("ERR SET CONF %s\n", dev);
		px4_close(fd);
		return -1;
	}

	return fd;
}

int send_bytes(int fd,const void *data, size_t len)
{
	// TODO: This could be way more efficient
	//       Using interrupts/DMA/polling would be much better
	for (size_t i = 0; i < len; i++) {
		// Block until we can send a byte
		write(fd, ((const uint8_t *)data) + i, 1);

		usleep(100);
	}

	return 0;
}
int hello_main(int argc, char *argv[])
{
	PX4_INFO("Hello, I am a dynamically loaded module.");

	PX4_INFO("Argv:");

	for (int i = 0; i < argc; ++i) {
		PX4_INFO("  %d: %s", i, argv[i]);
	}

	int fd = open_serial("/dev/ttyS3");
	//const char *data = "Hello abcefweffweefwfwefwef1112233";
	const float data = 123.4568f;
	int bytes_written = send_bytes(fd, &data, sizeof(data));
	//int bytes_written = write(fd, data, sizeof(data));
	PX4_INFO("SENDLEN=%i",bytes_written);
	return 0;
}
